//#**************************************************************
//#
//# filename:             MBSload.cpp
//#
//# author:               Gerstmayr Johannes
//#
//# generated:						17.October 2004
//# description:          Driver and model for timeintegration
//#                       Model of a rigid arm and a hydraulic zylinder
//# remarks:						  
//#
//# Copyright (c) 2003-2013 Johannes Gerstmayr, Linz Center of Mechatronics GmbH, Austrian
//# Center of Competence in Mechatronics GmbH, Institute of Technical Mechanics at the 
//# Johannes Kepler Universitaet Linz, Austria. All rights reserved.
//#
//# This file is part of HotInt.
//# HotInt is free software: you can redistribute it and/or modify it under the terms of 
//# the HOTINT license. See folder 'licenses' for more details.
//#
//# bug reports are welcome!!!
//# WWW:		www.hotint.org
//# email:	bug_reports@hotint.org or support@hotint.org
//#**************************************************************

#include "element.h"
#include "mbsload.h"
#include "elementdataaccess.h"
#include "constraint.h"
#include "sensors.h"
#include "control.h"
#include "body3d.h"
#include "body2d.h"
#include "mass1d.h"

MBSLoad::MBSLoad(): /*data(),*/ steps()
{
	val=0; gc=0; loadtype=TGCload; specpos = 0; /*el=NULL; */loadfunc = TLoadValue;
	forcepos = Vector3D(0,0,0); vecval = Vector3D(0,0,0);
	IOelement = 0; IOelement_outputnum = 0; //loadsteps initialized by 0 by default
	mathfunc.Init();
	node_1 = 0; node_2 = 0; node_f = 0; // DR  initialize by 0
	dim = 0;	//$ DR 2012-10
	SetLoadName("Load");//$ DR 2012-10
	localBodyAxis = 1; // SM 2012-01-11
};


mystr MBSLoad::LoadName() const 
{
	return loadname;
}

mystr MBSLoad::GetLoadTypeString() const
{
	switch(LoadType()) 
	{
		//the following names MUST coincide with the type names defined in MBS::MBS() !!!!
	case TGCload:						return "GCLoad";				break;
	case Tbodyload:					return "BodyLoad";			break;
	case Tsurfaceload:			return "SurfaceLoad";		break;
	case Tgravity:					return "Gravity";				break;
	case Tforcevector3D:		return "ForceVector3D"; break; //$ DR 2012-10
	case Tforcevector2D:		return "ForceVector2D"; break; //$ DR 2012-10
		//case Tpointload: 
		//	{
		//		if (el->Dim() == 3)	return "ForceVector3D"; 
		//		else	return "ForceVector2D"; 
		//		break;
		//	}
	case Tmomentvector3D:		return "MomentVector3D"; break; //$ DR 2012-10
	case Tmomentvector2D:		return "Moment2D";			break; //$ DR 2012-10
		//case Tpointmoment: 
		//	{
		//		if (el->Dim() == 3)	return "MomentVector3D"; 
		//		else	return "Moment2D"; 
		//		break;
		//	}
	case Tsurfacepressure:	return "SurfacePressure"; break;
	case Tcentrifugal:			return "CentrifugalLoad";		break;
	default:								return "Load";
	}
}

//$ DR 2012-10
// documentation of the LoadTypes, used for autogenerated docu
mystr MBSLoad::GetLoadTypeTexDescription() const
{
	mystr descr="";
	switch(LoadType()) 
	{
		//the following names MUST coincide with the type names defined in MBS::MBS() !!!!
	case TGCload:						
		descr = "A load acting on a generalized coordinate (gc) of the element.";
		break;
	case Tbodyload:			
		descr = "The load value is integrated over the volume of the body and applied to the body in the specified direction. For the case of a rigid body, a force of size load\\_value = density*gravity\\_constant applies a force according to the gravitational force.";
		break;
	case Tsurfaceload:				
		descr = "";	// description is still missing
		break;
	case Tgravity:							
		descr = "The load is integrated over the volume of the body and applied to the body in the specified direction. The density of the body is used to compute the force.";
		break;
	case Tforcevector3D:				
		descr = "A load acting on an element at a specified (local) position.";	
		break;
	case Tforcevector2D:					
		descr = "";	// description is still missing
		break;
	case Tmomentvector3D:					
		descr = "A torque acting on an element at a specified (local) position. ";
		break;
	case Tmomentvector2D:				
		descr = "";	// description is still missing
		break;
	case Tsurfacepressure:				
		descr = "";	// description is still missing
		break;
	case Tcentrifugal:				
		descr = "";	// description is still missing
		break;
	}

	return descr;
}

void MBSLoad::GetElementData(ElementDataContainer& edc) 		//fill in all element data
{
	Vector3D force;
	Vector3D pos;
	Vector2D force2;
	Vector2D pos2;
	
	ElementData ed;

	mystr eleminfo = LoadName();
	
	ed.SetText(LoadName(), "name"); ed.SetToolTipText("name of the load"); edc.Add(ed);
	ed.SetText(GetLoadTypeString(), "load_type"); ed.SetToolTipText("specification of load type. Once the load is added to the mbs, you MUST NOT change this type anymore!"); edc.Add(ed);
	ed.SetInt(GetOwnNum(), "load_number"); ed.SetToolTipText("number of the load in the mbs"); ed.SetLocked(1); edc.Add(ed);

	switch(LoadType())
	{
	case TGCload: 
		{
			double val;
			int gc;
			GetGCLoad(val, gc);
			ed.SetInt(gc, "generalized_coordinate");	ed.SetToolTipText("(local) number of the generalized coordinate"); edc.Add(ed);
			ed.SetDouble(val, "load_value");					ed.SetToolTipText("value of the load acting in the direction of generalized_coordinate"); edc.Add(ed);
			break;
		}
	case Tbodyload: 
		{
			double val;
			int dir;
			GetBodyLoad(val, dir);
			ed.SetInt(dir, "direction", 1,Dim());			ed.SetToolTipText("direction of the load"); edc.Add(ed);
			ed.SetDouble(val, "load_value");					ed.SetToolTipText("value of the load acting"); edc.Add(ed);
			break;
		}
	case Tforcevector2D: 
		{
			GetForceVector2D(force2, pos2);
			SetElemDataVector2D(edc, force2, "force_vector","defines the magnitude and direction of the force");
			SetElemDataVector2D(edc, pos2, "position","(local) position where the force is applied to the element");
			ed.SetInt(localBodyAxis, "local_force", 0,1);			ed.SetToolTipText("flag which describes, if local or global coordinate system is used: 1 = force in local body coordinate system, 0 = global force"); edc.Add(ed);
			break;
		}
	case Tforcevector3D: 
		{
			GetForceVector3D(force, pos);
			SetElemDataVector3D(edc, force, "force_vector","defines the magnitude and direction of the force");
			SetElemDataVector3D(edc, pos, "position","(local) position where the force is applied to the element");
			ed.SetInt(localBodyAxis, "local_force", 0,1);			ed.SetToolTipText("flag which describes, if local or global coordinate system is used: 1 = force in local body coordinate system, 0 = global force"); edc.Add(ed);
			break;
		}
	case Tmomentvector2D: 
		{
			double moment;
			GetMomentVector2D(moment, pos2);
			ed.SetDouble(val, "moment");							ed.SetToolTipText("magnitude of the moment");edc.Add(ed);
			SetElemDataVector2D(edc, pos2, "position","(local) position where the moment is applied to the element");
			break;
		}
	case Tmomentvector3D: 
		{
			GetMomentVector3D(force, pos);
			SetElemDataVector3D(edc, force, "moment_vector","defines the magnitude and direction of the moment");
			SetElemDataVector3D(edc, pos, "position","(local) position where the moment is applied to the element");
			ed.SetInt(localBodyAxis, "local_moment", 0,1);			ed.SetToolTipText("flag which describes, if local or global coordinate system is used: 1 = moment in local body coordinate system, 0 = global moment"); edc.Add(ed);
			break;
		}
		//case Tpointload: 
		//	{
		//		if (el->Dim() == 2)
		//		{
		//			GetForceVector2D(force2, pos2);
		//			SetElemDataVector2D(edc, force2, "Force_vector");
		//			SetElemDataVector2D(edc, pos2, "Position");
		//		}
		//		else
		//		{
		//			GetForceVector3D(force, pos);
		//			SetElemDataVector3D(edc, force, "Force_vector");
		//			SetElemDataVector3D(edc, pos, "Position");
		//		}
		//		break;
		//	}
		//case Tpointmoment: 
		//	{
		//		if (el->Dim() == 2)
		//		{
		//			double moment;
		//			GetMomentVector2D(moment, pos2);
		//			ed.SetDouble(val, "Moment"); edc.Add(ed);
		//			SetElemDataVector2D(edc, pos2, "Position");
		//		}
		//		else
		//		{
		//			GetMomentVector3D(force, pos);
		//			SetElemDataVector3D(edc, force, "Moment_vector");
		//			SetElemDataVector3D(edc, pos, "Position");
		//		}
		//		break;
		//	}
	case Tgravity: 
		{
			double val;
			int dir;
			GetGravity(val, dir);
			ed.SetInt(dir, "direction", 1, Dim());		ed.SetToolTipText("global direction of the gravity");edc.Add(ed);
			ed.SetDouble(val, "gravity_constant");		ed.SetToolTipText("use negative sign if necessary");edc.Add(ed);
			break;
		}
	case Tsurfaceload: 		
		{
			break;
		}
	case Tsurfacepressure: 
		{
			double val;
			int dir;
			GetSurfacePressure(val, dir);
			ed.SetInt(dir, "direction", 1, 2*Dim());	ed.SetToolTipText("local surface (inner/outer)");		edc.Add(ed);
			ed.SetDouble(val, "surface_pressure");	ed.SetToolTipText("use negative sign if necessary");	edc.Add(ed);
			break;
		}
	case Tcentrifugal:
		{
			GetCentrifugal(force,pos);
			SetElemDataVector3D(edc, force, "angular_velocity_vector","direction is the rotation axis and magnitude the magnitude of the angular velocity");
			SetElemDataVector3D(edc, pos, "point_on_axis", "one point on the rotation axis");
			break;
		}
	default: ;
	}

	//$ DR 2012-10:[ changed all to MathFunction, old code
	//double omega = 0;
	//double phase = 0;
	//double ts = 0, te = 0;
	//Vector v(5);
	//mystr loadname = "constant";

	//if (loadfunc == 2)
	//{
	//	GetHarmonic(omega, phase);
	//	loadname = "harmonic";
	//}
	//else if (loadfunc == 3)
	//{
	//	GetTimeSpan(ts, te);
	//	loadname = "time_span";
	//}
	//else if (loadfunc == 4)
	//{
	//	GetTimeRamp(v(1), v(2), v(3), v(4), v(5));
	//	loadname = "time_ramp";
	//}
	//else if (loadfunc == 6)
	//{
	//  loadname = "math_func";

	//ed.SetText(loadname.c_str(), "Time_dependency"); ed.SetToolTipText("{constant | harmonic | time_span | time_ramp}"); edc.Add(ed);
	//SetElemDataVector2D(edc, Vector2D(omega, phase), "Harmonic"); edc.Get(edc.Length()).SetToolTipText("[omega, phase(rad)]");
	//SetElemDataVector2D(edc, Vector2D(ts, te), "Time_span"); edc.Get(edc.Length()).SetToolTipText("[t_start, t_end]");
	//ed.SetVector(v.GetVecPtr(), 5, "Time_ramp"); ed.SetToolTipText("[magnitude, ramp_start, ramp_end, ramp2_start=-1, ramp2_end=-1]"); edc.Add(ed);

	//$ DR 2012-10:] changed all to MathFunction, old code

	ed.SetInt(loadfunc, "load_function_type", 0,2);			ed.SetToolTipText("time dependency of the load: 0..constant, 1..MathFunction, 2..IOElement"); edc.Add(ed);
	
	// MathFunction
	ElementDataContainer edc_mf;
	mathfunc.GetElementData(edc_mf);
	ed.SetEDC(&edc_mf,"MathFunction"); ed.SetToolTipText("MathFunction is used, if load_function_type = 1"); edc.Add(ed);	

	// IOelement
	ElementDataContainer edc_io;
	ed.SetInt(IOelement, "input_element_number");							ed.SetToolTipText("number of IOElement in the mbs");  edc_io.Add(ed);
	ed.SetInt(IOelement_outputnum, "input_local_number");			ed.SetToolTipText("number of output of IOElement connected to this element");  edc_io.Add(ed);
	ed.SetEDC(&edc_io,"IOElement"); ed.SetToolTipText("IOElement is used, if load_function_type = 2"); edc.Add(ed);	

	//GetElementDataAuto(edc);
}

int MBSLoad::SetElementData(ElementDataContainer& edc) 		//update element data
{
	int rv = 1;

	Vector2D force2;
	Vector2D pos2;

	GetElemDataText(mbs,edc,"name",loadname,1);

	mystr type_old = GetLoadTypeString();
	mystr type_new = edc.TreeGetString("load_type");
	if(!type_new.Compare(type_old))
	{
		GetMBS()->UO().InstantMessageText("ERROR: You MUST NOT change the type of the load!");
		return 0;
	}

	switch(LoadType())
	{
	case TGCload: 
		{
			GetElemDataInt(mbs, edc, "generalized_coordinate", gc, 1);
			GetElemDataDouble(mbs, edc, "load_value", val, 1);
			break;
		}
	case Tbodyload: 
		{
			GetElemDataInt(mbs, edc, "direction", specpos, 1);
			GetElemDataDouble(mbs, edc, "load_value", val, 1);
			break;
		}
	case Tforcevector2D: 
		{
			GetElemDataVector2D(mbs, edc, "force_vector", force2, 1);
			GetElemDataVector2D(mbs, edc, "position", pos2, 1);
			GetElemDataInt(mbs, edc, "local_force", localBodyAxis, 1);
			SetForceVector2D(force2, pos2,localBodyAxis);
			break;
		}
	case Tforcevector3D: 
		{
			GetElemDataVector3D(mbs, edc, "force_vector", vecval, 1);
			GetElemDataVector3D(mbs, edc, "position", forcepos, 1);
			GetElemDataInt(mbs, edc, "local_force", localBodyAxis, 1);
			break;
		}
	case Tmomentvector2D: 
		{
			double moment;
			GetElemDataDouble(mbs, edc, "moment", val, 1);
			GetElemDataVector2D(mbs, edc, "position", pos2, 1);
			SetMomentVector2D(val, pos2);
			break;
		}
	case Tmomentvector3D: 
		{
			GetElemDataVector3D(mbs, edc, "moment_vector", vecval, 1);
			GetElemDataVector3D(mbs, edc, "position", forcepos, 1);
			GetElemDataInt(mbs, edc, "local_moment", localBodyAxis, 1);
			break;
		}
	case Tgravity: 
		{
			GetElemDataInt(mbs, edc, "direction", specpos, 1);
			GetElemDataDouble(mbs, edc, "gravity_constant", val, 1);
			break;
		}
	case Tsurfaceload: 		
		{
			break;
		}
	case Tsurfacepressure: 
		{
			GetElemDataInt(mbs, edc, "direction", specpos, 1);
			GetElemDataDouble(mbs, edc, "surface_pressure", val, 1);
			break;
		}
	case Tcentrifugal:
		{
			GetElemDataVector3D(mbs, edc, "angular_velocity_vector", vecval, 1);
			GetElemDataVector3D(mbs, edc, "point_on_axis", forcepos, 1);
			break;
		}
	default: ;
	}

	//$ DR 2012-10:[ changed all to MathFunction, old code
	//mystr str;
	//Vector2D v2;
	//Vector v5(5);

	//if (GetElemDataText(mbs, edc, "Time_dependency", str, 0))
	//{
	//	if (str == mystr("harmonic"))
	//	{
	//		if (GetElemDataVector2D(mbs, edc, "Harmonic", v2, 1))
	//		{
	//			SetHarmonic(v2(1), v2(2));
	//		}
	//	}
	//	else if (str == mystr("time_span"))
	//	{
	//		if (GetElemDataVector2D(mbs, edc, "Time_span", v2, 1))
	//		{
	//			SetTimeSpan(v2(1), v2(2));
	//		}
	//	}
	//	else if (str == mystr("time_ramp"))
	//	{
	//		if (GetElemDataVector(mbs, edc, "Time_ramp", v5, 1) && v5.Length() == 5)
	//		{
	//			SetTimeRamp(v5(1), v5(2), v5(3), v5(4), v5(5));
	//		}
	//	}
	//}
	//$ DR 2012-10:] changed all to MathFunction, old code

	int lf;
	GetElemDataInt(mbs,edc,"load_function_type",lf,1);
	loadfunc = (TMBSLoadFunc) lf;

	if(loadfunc == TLoadMathFunc)
	{
		int idx = edc.Find(mystr("MathFunction"));
		if(idx && (edc.Get(idx)).IsEDC())
		{
			//const ElementDataContainer* edcp_mf = (edc.Get(idx)).GetEDC();
			//const ElementDataContainer& edc_mf = (const ElementDataContainer&) *edcp_mf;
			//mathfunc.SetElementData(mbs,edc_mf);
			ElementDataContainer* edcp_mf = (edc.Get(idx)).GetEDC();
			ElementDataContainer edc_mf = *edcp_mf;
			mathfunc.SetElementData(mbs,edc_mf);	//$ DR 2012-12-12
		}
	}

	if(loadfunc == TLoadIOElement)
	{
			GetElemDataInt(mbs,edc,"IOElement.input_element_number",IOelement,1);
			GetElemDataInt(mbs,edc,"IOElement.input_local_number",IOelement_outputnum,1);
	}

	//SetElementDataAuto(edc);
	return rv;
}

int MBSLoad::CheckConsistency(Element& el, mystr& errorstr) //rv==0 --> OK, rv==1 --> can not compute, rv==2 --> can not draw and not compute
{
	int rv = 0;

	if(el.Dim()!=Dim())
	{
		errorstr += mystr("    The dimension of element ") + mystr(el.GetOwnNum()) + mystr(" and the applied load ") +mystr(GetOwnNum())+ mystr(" are different!\n");
		rv = Maximum(rv,1); 
	}

	switch(LoadType())
	{
	case TGCload: 
		{
			if(el.SS() < gc || 0 > gc)
			{
				errorstr += mystr("    The generalized coordinate of load ") +mystr(GetOwnNum()) + mystr(" is not fitting to the number of equations of element ") + mystr(el.GetOwnNum()) + mystr("!\n");
				rv = Maximum(rv,1); 
			}
			break;
		}
	case Tbodyload: 
		{
			break;
		}
	case Tforcevector2D: 
		{
			break;
		}
	case Tforcevector3D: 
		{
			break;
		}
	case Tmomentvector2D: 
		{
			break;
		}
	case Tmomentvector3D: 
		{
			break;
		}
	case Tgravity: 
		{
			break;
		}
	case Tsurfaceload: 		
		{
			break;
		}
	case Tsurfacepressure: 
		{
			break;
		}
	case Tcentrifugal:
		{
			break;
		}
	default: ;
	}

	return rv;
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++        Get Force type and values       +++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


void MBSLoad::GetGCLoad(double& vali, int& gci)
{
	vali=val;
	gci =gc;
}

void MBSLoad::GetBodyLoad(double& vali, int& dir)
{
	vali=val; 
	dir = specpos;
}

void MBSLoad::GetGravity(double& gravityconstant, int& dir)
{
	gravityconstant=val; 
	dir = specpos;
}

void MBSLoad::GetForceVector3D(Vector3D & force, Vector3D& pos)
{
	//if(!localBodyAxis)
	//{
		force = vecval;
		pos = forcepos;
	//}
	//else // SM 2013-01-11: TODO --> not implemented for this case
	//{
	//	force = Vector3D(0.);
	//	pos = Vector3D(0.);
	//}
}

void MBSLoad::GetMomentVector3D(Vector3D & moment, Vector3D& pos)
{
	//if(!localBodyAxis)
	//{
		moment = vecval;
		pos = forcepos;
	//}
	//else // SM 2013-01-11: TODO --> not implemented for this case
	//{
	//	moment = Vector3D(0.);
	//	pos = Vector3D(0.);
	//}

}

void MBSLoad::GetForceVector2D(Vector2D & force, Vector2D& pos)
{
	//if(!localBodyAxis)
	//{
		force.X() = vecval.X();
		force.Y() = vecval.Y();
		pos.X() = forcepos.X();
		pos.Y() = forcepos.Y();
	//}
	//else // SM 2013-01-11: TODO --> not implemented for this case
	//{
	//	force.X() = 0;
	//	force.Y() = 0;
	//	pos.X() = 0;
	//	pos.Y() = 0;
	//}
}

void MBSLoad::GetMomentVector2D(double& moment, Vector2D& pos)
{
	//if(!localBodyAxis)
	//{
		moment = vecval.Z();
		pos.X() = forcepos.X();
		pos.Y() = forcepos.Y();
	//}
	//else // SM 2013-01-11: TODO --> not implemented for this case
	//{
	//	moment = 0;
	//	pos.X() = 0;
	//	pos.Y() = 0;
	//}
}

void MBSLoad::GetCentrifugal(Vector3D & omega, Vector3D& pos)
{
	omega = vecval;
	pos = forcepos;
}

void MBSLoad::GetContactZ(double& zpos)
{
	zpos = val ;
}

void MBSLoad::GetSurfacePressure(double& pressure, int& surfacenum)
{
	pressure = val; 
	surfacenum = specpos;
}

void MBSLoad::GetRotatingForce(double& vali, int& node_force, int& node_ax1, int& node_ax2)
{
	vali = val;
	node_force = node_f;
	node_ax1 = node_1;
	node_ax2 = node_2;

	//$ DR 2011-04-14: old code:
	// specpos is used to store node number of node where force is applied
	//node_force = specpos;
	//// IOelement and IOelement_outputnum are used to store node number of axis nodes
	//node_ax1 = IOelement;
	//node_ax2 = IOelement_outputnum;
}

void MBSLoad::GetParameterInt(int* & p_inti)
{
	p_inti = p_int;
}

void MBSLoad::GetParameterDouble(double* & p_doublei)
{
	p_doublei = p_double;
}

void MBSLoad::GetParameterVector3D(double* & p_vector3di)
{
	p_vector3di = p_vector3di;
}

//$ DR 2012-10:[ changed all to MathFunction, old code
//void MBSLoad::GetHarmonic(double& omega_i, double& phase_i) 
//{
//	omega_i = data(1);
//	phase_i = data(2);
//}
//
//void MBSLoad::GetTimeSpan(double& ts, double& te)
//{
//	ts = data(1);
//	te = data(2);
//}
//
//void MBSLoad::GetTimeRamp(double& mag, double& t1on, double& t2on, double& t1off, double& t2off)
//{
//	mag  = data(1);
//	t1on = data(2);
//	t2on = data(3);
//	t1off= data(4);
//	t2off= data(5);
//}
//$ DR 2012-10:] changed all to MathFunction, old code

void MBSLoad::GetMathFunction(MathFunction& userfunction)
{
	userfunction = mathfunc;
}


//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+++++++++++++++++++++++++++++        Set Force type and values       +++++++++++++++++++++++++++++++++++++++++++
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

void MBSLoad::SetGCLoad(double vali, int gci)
{
	val=vali; gc=gci;
	loadtype = TGCload;
}

void MBSLoad::SetBodyLoad(double vali, int dir)
{
	val=vali; specpos = dir;
	if (specpos > 3) {GetMBS()->UO() << "ERROR: SetBodyLoad only accepts dir = 1 ... 3\n"; specpos = 3;}
	loadtype = Tbodyload;
}

void MBSLoad::SetGravity(double gravityconstant, int dir)
{
	val=gravityconstant; specpos = dir;
	if (specpos > 3) {GetMBS()->UO() << "ERROR: SetGravity only accepts dir = 1 ... 3\n"; specpos = 3;}
	loadtype = Tgravity;
}

void MBSLoad::SetForceVector3D(const Vector3D & force, const Vector3D& pos, const int useLocalBodyAxis)
{
	val=1.;
	loadtype = Tforcevector3D;
	vecval = force;
	forcepos = pos;
	node_f = 0;	// DR 2011-05-18
	localBodyAxis = useLocalBodyAxis; // SM 2013-01-11
}

void MBSLoad::SetForceVector3D(const Vector3D & force, const int& nodenr, const int useLocalBodyAxis)
{
	val=1.;
	loadtype = Tforcevector3D;
	vecval = force;
	forcepos = Vector3D(0.0);
	node_f = nodenr;
	localBodyAxis = useLocalBodyAxis; // SM 2013-01-11
}

void MBSLoad::SetContactZ(double zpos)
{
	val = zpos;
	loadtype = TcontactZ;
	specpos = 3; //z-dir
}

void MBSLoad::SetMomentVector3D(const Vector3D & moment, const Vector3D& pos, const int useLocalBodyAxis)
{
	val=1.;
	loadtype = Tmomentvector3D;
	vecval = moment;
	forcepos = pos;
	localBodyAxis = useLocalBodyAxis; // SM 2013-01-11
}

void MBSLoad::SetForceVector2D(const Vector2D & force, const Vector2D& pos, const int useLocalBodyAxis)
{
	val=1.;
	loadtype = Tforcevector2D;
	vecval.X() = force.X();
	vecval.Y() = force.Y();
	forcepos.X() = pos.X();
	forcepos.Y() = pos.Y();
	localBodyAxis = useLocalBodyAxis; // SM 2013-01-11
}

void MBSLoad::SetMomentVector2D(double moment, const Vector2D& pos)
{
	val=1.;
	loadtype = Tmomentvector2D;
	vecval.Z() = moment;
	forcepos.X() = pos.X();
	forcepos.Y() = pos.Y();
}

void MBSLoad::SetSurfacePressure(double pressure, int surfacenum)
{
	val = pressure; 
	specpos = surfacenum;
	loadtype = Tsurfacepressure;
}

void MBSLoad::SetDistributedMoment2D(double moment)
{
	val = moment;
	loadtype = Tdistributedmoment;
}

void MBSLoad::SetCentrifugalLoad(const Vector3D& omega, const Vector3D& pos)
{
	val = 1.;
  vecval = omega;
	forcepos = pos;
	loadtype = Tcentrifugal;
}

// ATTENTION: it is assumed, that the nodes are local node_numbers of the SAME Element (therefore only for CMS-element and equivalents)
void MBSLoad::SetRotatingForce(double vali, int node_force, int node_ax1, int node_ax2)
{
	val = vali;
	node_f = node_force;
	node_1 = node_ax1;
	node_2 = node_ax2;
	loadtype = Trotatingforce;
	
	//$ DR 2011-04-14: old code:
	// specpos is used to store node number where nodal force is applied
	//specpos = node_force;
	//// IOelement and IOelement_outputnum are used to store node numbers for axis
	//IOelement = node_ax1;
	//IOelement_outputnum = node_ax2;
}

void MBSLoad::SetParameterInt(int vali, int* p_inti)
{
	loadtype = Tparameterint;
	val = vali;
	p_int = p_inti;
}

void MBSLoad::SetParameterDouble(double vali, double* p_doublei)
{
	loadtype = Tparameterdouble;
	val = vali;
	p_double = p_doublei;
}

void MBSLoad::SetParameterVector3D(Vector3D& vecvali, Vector3D* p_vector3di)
{
	val = 1.;
	loadtype = Tparametervector3d;
	vecval = vecvali;
	p_vector3d = p_vector3di;
}

void MBSLoad::SetHarmonic(double omega_i, double phase_i) 
{
// alternative (depreciated feature of mathfunction
//	mathfunc.SetHarmonic(Vector(omega_i),Vector(phase_i),Vector(1.));
//
	mystr expr = mystr("sin(")+ mystr(omega_i) + mystr("*t)");
	//CMBSParser parser;

	mathfunc.SetExpression(expr,"t",mbs);

	loadfunc = TLoadMathFunc;
	
	//$ DR 2012-10: changed all to MathFunction, old code
	//data.Flush();
	//data.Add(omega_i);
	//data.Add(phase_i);
	//loadfunc = 2; 
}

void MBSLoad::SetTimeSpan(double ts, double te)
{
	mathfunc.SetPiecewise(Vector(0.,ts,te,0.),Vector(0.,1.,1.,0.),0);
	loadfunc = TLoadMathFunc;

	//$ DR 2012-10: changed all to MathFunction, old code
	//data.Flush();
	//data.Add(ts);
	//data.Add(te);
	//loadfunc = 3; 
}

void MBSLoad::SetTimeRamp(double mag, double t1on, double t2on, double t1off, double t2off)
{
	val = 1;	//$ DR 2013-01-29: change log 372, if MathFunction or IOElement are used val=1
	if(t1off == -1)
	{
		mathfunc.SetPiecewise(Vector(0.,t1on,t2on,t2on+1),Vector(0.,0.,mag,mag),1);
	}
	else
	{
		if(t2off == -1)
		{
			mathfunc.SetPiecewise(Vector(0.,t1on,t2on,t2on+1),Vector(0.,0.,mag,mag),1);
		}
		else
		{
			mathfunc.SetPiecewise(Vector(0.,t1on,t2on,t1off,t2off,t2off+1),Vector(0.,0.,mag,mag,0.,0.),1);
		}
	}
	loadfunc = TLoadMathFunc;
	//$ DR 2012-10: changed all to MathFunction, old code
	//data.Flush();
	//data.Add(mag);
	//data.Add(t1on);
	//data.Add(t2on);
	//data.Add(t1off);
	//data.Add(t2off);
	//loadfunc = 4; 
}

void MBSLoad::SetIOElement(int IOelementI, int IOelement_outputnumI)
{
	loadfunc = TLoadIOElement; //$ DR 2012-10: changed loadfunc, IOElement changed from 5 to 2
	val = 1;	//$ DR 2013-01-29: change log 372, if MathFunction or IOElement are used val=1
	IOelement = IOelementI;
	IOelement_outputnum = IOelement_outputnumI;
}

void MBSLoad::SetMathFunction(MathFunction userfunction)
{
	//data.Flush();
	mathfunc = userfunction;
	loadfunc = TLoadMathFunc; //$ DR 2012-10: changed loadfunc, MathFunction changed from 6 to 1
	val = 1;	//$ DR 2013-01-29: change log 372, if MathFunction or IOElement are used val=1
}

void MBSLoad::SetLoadSteps(const TArray<StepSettings>& settings)
{
	steps = settings;
}

int MBSLoad::GetCStepNumber(double t) const
{ 
	return mbs->GetCSNumber(t); // do indeed calculate
//	return el->GetMBS()->ComputationStepNumber(); // access-function: gives wrong values for t=0 during assemble
}

double MBSLoad::GetCStepTime(double t) const
{ 
	return mbs->GetCSTime(t); // do indeed calculate
//	return el->GetMBS()->ComputationStepTime(); // access-function: gives wrong values for t=0 during assemble
}

 int MBSLoad::GetRampMode(int i) const
{ 
	return steps(i).GetRampMode(); 
}

double MBSLoad::GetFinalValue(int i) const
{ 
	return steps(i).GetLoadFactor(); 
}

int const MBSLoad::NLoadSteps() const 
{ 
	return steps.Length(); 
}

// Get load factor from loadsteps array
double MBSLoad::GetCStepsFact(double t) const
{
	if(t <= 0) 
		return 0.;
	
  double steptime = GetCStepTime(t);    // step number (computation step number from TimeInt)
	int stepnumber = GetCStepNumber(t);   // step time   (computation step time from TimeInt)

// return value when time is after last computation step endtime...
	if(stepnumber < 0 && t > 0.) 
		return GetFinalValue(steps.Length());

// return value when too few load steps are defined
	if(stepnumber > NLoadSteps()) 
		return GetFinalValue(steps.Length());


	double finalvalue = GetFinalValue(stepnumber); // value at the end of this interval
// return value for STEP ( finalvalue is valid for entire interval )
	if(GetRampMode(stepnumber) == TCSRStep)
		return finalvalue; 

	double startvalue = 0.;  // value at beginning of this interval, =0. means that all constraints start OFF !!!
	if (stepnumber != 1) 
    startvalue = GetFinalValue(stepnumber-1);
// return value for LINEAR ( linear interpolation between beginning and end of the interval )
  if(GetRampMode(stepnumber) == TCSRLinear)
	{
		double stepfactor = startvalue + (finalvalue-startvalue)*steptime;
		return stepfactor;
	}

	const double eps_t = 1e-12;
// return value for EXPONENTIAL ( eponential interpolation between beginning and end of the interval )
	if(steptime < eps_t) 
		return startvalue;
	else if(steptime == 1.)
		return finalvalue;
	else
	{
		double factor_growth_decay = 1.;
		if(finalvalue < startvalue) 
			factor_growth_decay=-1;
// exponential interpolation:  y(x) = y(0) * (y(1) / y(0))^x ... with y(0) and y(1) may not be 0.
// with y(x) = a*e^+x + d , x[0..1] --> y(0) = a+d, y(1) = ae+d    --> a = (y(1)-y(0)) / (e-1),    d = y(0) - a  (growth)
// with y(x) = a*e^-x + d , x[0..1] --> y(0) = a+d, y(1) = ae^-1+d --> a = (y(1)-y(0)) / (e^-1-1), d = y(0) - a  (decay)
		double a = (finalvalue-startvalue) / (exp(factor_growth_decay)-1.);
		double d = startvalue - a;
		double stepfactor = a * exp(steptime*factor_growth_decay) + d;
		return stepfactor;
	}
  return 0.;
}

double MBSLoad::Evaluate(double t) const
{
	// special cases: -------------------------
	//$ AD 2011-09-15: draw all loads before computation starts (even if they are off at t=0)
	if(! mbs->GetSimulationStatus().GetStatusFlag(TSimulationNotStarted) && t == 0.)   //$ MaSch 2013-08-19
		return val;

	//$ AD 2011-10-28: all parameter loads are not touched at t=0;
	if (t==0. && ( loadtype == Tparameterdouble || loadtype == Tparametervector3d) )
		return val;

	// general cases: -------------------------
	// get loadstep (static computation)
	double loadstep = 1.0;		// range 0..1, used for static computations

	if (NLoadSteps()==0)
	{
		if(GetCStepNumber(t) == 1)
		{
			loadstep = mbs->LoadFact();        // linear turn on in first CS Interval
		}
	}
	else
	{ 
	 loadstep = GetCStepsFact(t);             
	}

	// get loadfunc (static+dynamic computation)
	if (!loadfunc)				// no Mathfunction and no IOElement used
	{
		return val*loadstep;
	}
	else
	{
		switch (loadfunc)
		{
		case TLoadMathFunc: 				
			{
				return val * loadstep * mathfunc.Evaluate(t);
				break;
			}
		case TLoadIOElement:		// loadsteps are not used if IOElements are used
			{
				const InputOutputElement& ioe = (const InputOutputElement&)(mbs->GetElement(IOelement));
				return val * ioe.GetOutput(t, IOelement_outputnum);
				break;
			}
		}
	}
	return 0;
}

//double MBSLoad::Evaluate(double t) const
//{
//	if(! mbs->GetSimulationStatus() == TSimulationNotStarted && t == 0.)   //$ AD 2011-09-15: draw all loads before computation starts (even if they are off at t=0)
//		return val;
//	//    ------ XXXXXXXX --------
//	double vv;
////	if (loadsteps.Length()==0)
//	if (NLoadSteps()==0)
//	{
//		if(GetCStepNumber(t) == 1)
//		{
//			vv = val*mbs->LoadFact();        // linear turn on in first CS Interval
//		}
//		else 
//		{
//			vv = val;                                 // full load after first CS Interval
//		}
//	}
//	else
//	{ 
//	 vv = val*GetCStepsFact(t);                   // 
//	}
//	//double vv = val*el->GetMBS()->LoadFact();
//
////$ AD 2011-10-28: all parameter loads are not touched ar t=0;
//	if (t==0. && ( loadtype == Tparameterdouble || loadtype == Tparametervector3d) )
//		return val;
//
//	if (!loadfunc) return vv;
//	else
//	{
//		switch (loadfunc)
//		{
//			//$ DR 2012-10:[ changed all to MathFunction, old code
//			//case 1: if (t < 1.) return 0.5*vv*(1.-cos(MY_PI*t)); else return vv; break;
//			//case 2: return vv*sin(data(1)*t+data(2)); break;
//			//case 3: if (t >= data(1) && t < data(2)) return vv; else return 0; break;
//			//case 4: //ramp with ton and tof
//			//	{
//			//		double mag = data(1)*vv;
//			//		if (t < data(2)) return 0; 
//			//		if (t < data(3)) return (t-data(2))/(data(3)-data(2))*mag; 
//			//		if (t < data(4) || data(4) == -1) return mag; 
//			//		if (t < data(5)) return (data(5)-t)/(data(5)-data(4))*mag;
//			//		return 0;
//			//		break;
//			//	}
//			//case 5:
//			//	{
//			//		const InputOutputElement& ioe = (const InputOutputElement&)(mbs->GetElement(IOelement));
//			//		//const InputOutputElement* ioe = (const InputOutputElement*)el->GetMBS()->GetElementPtr(IOelement);
//			//		double val = ioe.GetOutput(t, IOelement_outputnum);
//			//		return val;
//
//			//		break;
//			//	}
//			//case 6:
//			//	{
//			//		double val = mathfunc.Evaluate(t);
//			//		return val;
//			//	}
//			//default: ;
//			//$ DR 2012-10:] changed all to MathFunction, old code
//		case 1: 				
//			{
//				double val = mathfunc.Evaluate(t);
//				return val;
//			}
//		case 2:
//			{
//				const InputOutputElement& ioe = (const InputOutputElement&)(mbs->GetElement(IOelement));
//				double val = ioe.GetOutput(t, IOelement_outputnum);
//				return val;
//				break;
//			}
//		default: ;
//		}
//	}
//	return 0;
//}


void MBSLoad::AddElementLoad(Vector& f, double t, Element* el)
//void MBSLoad::AddElementLoad(Vector& f, double t) //$ DR 2012-10: loads moved from element to mbs, old code
{
	if (loadtype == TGCload)
	{
		f(gc) += Evaluate(t);
	} 
	else if (loadtype == Tbodyload || loadtype == Tgravity)
	{
		if (el->IsRigid())
		{
			dudq.SetLen(f.GetLen());
			if (specpos == 1){el->GetDuxDq(dudq);}
			else if (specpos == 2){el->GetDuyDq(dudq);}
			else if (specpos == 3){el->GetDuzDq(dudq);}

			if (loadtype==Tbodyload)
			{
				dudq *= Evaluate(t)*el->GetVolume();
			}
			else // Tgravity
			{
				dudq *= Evaluate(t)*el->GetMass();
			}
			f += dudq;
		}
		else
		{
			H.SetSize(f.Length(),el->Dim());
			if(loadtype == Tbodyload)
			{
				el->GetIntDuDq(H); //in fact it is DuDq Transposed
			}
			else // Tgravity
			{
				el->GetIntRhoDuDq(H); //in fact it is int_V rho DuDq Transposed
			}
			//GetMBS()->UO() << "H=" << H << "\n";
			if (el->Dim() == 3)
			{
				Vector3D l(0.,0.,0.);
				l(specpos) = Evaluate(t);
				Mult(H,l,dudq);
				f += dudq;
			}
			else
			{
				Vector2D l(0.,0.);
				if (specpos > 2) {GetMBS()->UO() << "ERROR: bodyload/gravity accepts only dir = 1..2 in 2D bodies!!!\n"; specpos = 2;}
				l(specpos) = Evaluate(t);
				Mult(H,l,dudq);
				f += dudq;
			}
		}
	}
	else if ((loadtype == Tforcevector2D) || (loadtype == Tforcevector3D)) 
	{
		//IF BODY IS RIGID
		//add force load in mass center
		dudq.SetLen(f.GetLen());

		if (!el->IsRigid())
		{
			if(el->Dim()==3)	// DR 2011-12-23: bugfix B033 --> not yet totally fixed, but working for node-nr
			{
				Body3D* bo = (Body3D*)el;
				H.SetSize(f.Length(),el->Dim());
				if(node_f)	// DR 2011-05-18: the constructor with node-nr is used
				{
					forcepos = el->GetNodePos(node_f);
					bo->GetNodedPosdqT(node_f, H);
				}
				else
				{
					bo->GetdPosdqT(forcepos, H);
				}
				Vector3D force = Evaluate(t)*vecval;
				if(localBodyAxis) // MSax: transform local force to global force
				{
					Matrix3D A = bo->GetRotMatrix(forcepos);
					force = A*force;
				}
				Mult(H, force, dudq);
				f += dudq;
			}
			else
			{
				Body2D* bo = (Body2D*)el;
				Vector2D force(Evaluate(t)*vecval.X(),Evaluate(t)*vecval.Y());

				if(localBodyAxis) // MSax: transform local force to global force
				{
					force = bo->GetRotMatrix2D()*force;
				}

				H.SetSize(f.Length(),el->Dim());

				bo->GetdPosdqT(Vector2D(forcepos.X(),forcepos.Y()), H);
				Mult(H, force, dudq);
				f += dudq;
			}
		}
		else
		{
			if(el->Dim()==3)
			{
				Body3D* bo = (Body3D*)el;
				Vector3D force = Evaluate(t)*vecval;

				if(localBodyAxis) // MSax: transform local force to global force
				{
					Matrix3D A = bo->GetRotMatrix(); // rigid body --> no local position needed
					force = A*force;
				}

				bo->ApplyDprefdq(dudq, force);
				f += dudq;
				//add load due to moments:
				if (forcepos.X() != 0 || forcepos.Y() != 0 || forcepos.Z() != 0)
				{
					//assert(forcepos.X() == 0 || forcepos.Y() == 0 || forcepos.Z() == 0);  //$ MaSch 2012-03: Whoever needed this, please tell JG why.

					Vector3D pglob = bo->GetRotMatrix()*forcepos;
					Vector3D mF = pglob.Cross(force);
					bo->ApplyDrotrefdq(dudq, mF);
					//el->UO() << "f_rotref=" << dudq << "\n";
					f += dudq;
				}
			}
			else if (el->Dim()==2)
			{
				Body2D* bo = (Body2D*)el;
				Vector2D force(vecval.X(),vecval.Y());
				force *= Evaluate(t);

				if(localBodyAxis) // MSax: transform local force to global force
				{
					force = bo->GetRotMatrix2D()*force;
				}

				bo->ApplyDprefdq(dudq, force);
				f += dudq;
				//add load due to moments:
				if (forcepos.X() != 0 || forcepos.Y() != 0 || forcepos.Z() != 0)
				{
					//assert(forcepos.X() == 0 || forcepos.Y() == 0 || forcepos.Z() == 0);  //$ MaSch 2012-03: Whoever needed this, please tell JG why.

					Vector2D pglob = bo->GetRotMatrix2D()*Vector2D(forcepos.X(),forcepos.Y());
					double mF = pglob.Cross(force); //Applied moment
					bo->ApplyDrotrefdq(dudq, mF);
					f += dudq;
				}
			}
			else if (el->Dim()==1)	//$ DR 2013-06-19
			{
				//$ DR 2013-06-24 removed again, according to JG
				//Mass1D* bo = (Mass1D*)el;
				//double force = vecval.X()*Evaluate(t);
				//bo->ApplyDprefdq(dudq, force);
				//f += dudq;
			}
		}
	}
	else if ((loadtype == Tmomentvector2D) || (loadtype == Tmomentvector3D)) 
	{
		//IF BODY IS RIGID
		//add force load in mass center
		dudq.SetLen(f.GetLen());
		if (el->IsRigid())
		{
			if (el->Dim() == 3)
			{
				Body3D* bo = (Body3D*)el;
				//el->UO() << "vecval=" << Evaluate(t)*vecval << "\n";

				Vector3D force = Evaluate(t)*vecval;
				if(localBodyAxis) // MSax: transform local force to global force
				{
					Matrix3D A = bo->GetRotMatrix();
					force = A*force;
				}

				bo->ApplyDrotrefdq(dudq, force);
				//el->UO() << "dudq=" << dudq << "\n";
				f += dudq;
			}
			else if (el->Dim() == 2)
			{
				Body2D* bo = (Body2D*)el;
				bo->ApplyDrotrefdq(dudq, Evaluate(t)*vecval.Z());
				f += dudq;
			}
		}
		else //flexible:
		{
			if(el->Dim()==3)
			{
				Body3D* bo = (Body3D*)el;

				Vector3D force = Evaluate(t)*vecval;
				if(localBodyAxis)
				{
					Matrix3D A = bo->GetRotMatrix(forcepos);
					force = A*force;
				}
			
				H.SetSize(f.Length(),el->Dim());
				bo->GetdRotdqT(forcepos, H);
				Mult(H, force, dudq);
				f += dudq;
			}
			else if(el->Dim()==2)
			{
				Body2D* bo = (Body2D*)el;
				double force = Evaluate(t) * vecval.Z();
				Vector2D fpos(forcepos.X(), forcepos.Y());
				H.SetSize(f.Length(),1);
				bo->GetdAngle2DdqT(fpos, H);
				for (int i=1; i <= f.Length(); i++)
				{
					f(i) += force*H(i,1);
				}
			}
		}
	}
	else if (loadtype == TcontactZ)
	{
		//for ANCF-plate elements only!!!
		//add force load in mass center
		dudq.SetLen(f.GetLen());

		Body3D* bo = (Body3D*)el;
		Vector3D force(0,0,1);

		double cstiff = 1e5;

		int np = 4;
		Vector3D lp[4];
		Vector3D size = bo->GetSize();
		double r = 0.3;//0.5/sqrt(3.);
		lp[0] = Vector3D(-r*size.X(),-r*size.Y(),0);
		lp[1] = Vector3D(-r*size.X(), r*size.Y(),0);
		lp[2] = Vector3D( r*size.X(),-r*size.Y(),0);
		lp[3] = Vector3D( r*size.X(), r*size.Y(),0);

		for (int i=0; i < np; i++)
		{
			double fdamp = -cstiff*0.005;
			Vector3D p = bo->GetPos(lp[i]);
			double gap = (p.Z()-val);
			if (gap <= 0)
			{

				Vector3D v = bo->GetVel(lp[i]);
				//double sgn = Sgn(fdamp);
				//if (fabs(fdamp)  > fabs(gap*cstiff)) fdamp = sgn*fabs(gap*cstiff);
				double fact = -(gap*cstiff-fdamp*v.Z()); //XG(1) = gap * cstiff  -> pressure is negative, XG(1) always negative!

				H.SetSize(f.Length(),el->Dim());

				bo->GetdPosdqT(lp[i], H);
				Mult(H, fact*force, dudq);
				f += dudq;
			}
		}
	}
	else if (loadtype == Tsurfacepressure)
	{
		//Add surface pressure
		el->AddSurfacePressure(f, Evaluate(t), specpos);
	}
	else if (loadtype == Tdistributedmoment)
	{
		if (el->Dim() == 2)
		{
			if (el->IsRigid())
			{
				GetMBS()->UO() << "Error: distributed moment only for 2D flexible beams!\n";
			}
			else
			{
				dudq.SetLen(f.Length());
				el->GetIntDkappaDq2D(dudq); //in fact it is DkappaDq Transposed

				dudq *= Evaluate(t);

				f += dudq;
			}
		}
		else
		{
			GetMBS()->UO() << "Error: distributed moment only for 2D flexible beams!\n";
		}
	}
	else if(loadtype == Tcentrifugal)
	{
		if (el->IsRigid())
		{
			if (el->Dim() == 3)
			{
				dudq.SetLen(f.Length());
				
				Vector3D p = el->GetPos(Vector3D(0.0,0.0,0.0)); // cm
				Vector3D r_rel = forcepos-p;
				Vector3D cforce = vecval.Cross(vecval.Cross(r_rel));

				el->GetDuxDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.X());
				f += dudq;

				el->GetDuyDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.Y());
				f += dudq;

				el->GetDuzDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.Z());
				f += dudq;
			}
			else if (el->Dim() == 2)
			{
				dudq.SetLen(f.Length());

				Vector3D p = el->GetPos(Vector3D(0.0,0.0,0.0)); // cm
				Vector3D r_rel = forcepos-p;
				Vector3D cforce = vecval.Cross(vecval.Cross(r_rel));

				el->GetDuxDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.X());
				f += dudq;

				el->GetDuxDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.X());
				f += dudq;

				el->GetDuyDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.Y());
				f += dudq;

				el->GetDuzDq(dudq);
				dudq *= (Evaluate(t)*el->GetMass()*cforce.Z());
				f += dudq;
			}
		}
		else 
		{
			if (el->Dim() == 3)
			{
				H.SetSize(f.Length(),el->Dim());
				el->GetIntDuDqFCentrifugal(H,vecval,forcepos);
				H *= Evaluate(t);
				
				Mult(H,Vector3D(1.0,1.0,1.0),dudq);
				f += dudq;
			}	
			else if (el->Dim() == 2)
			{
				H.SetSize(f.Length(),el->Dim());
				el->GetIntDuDqFCentrifugal(H,vecval,forcepos);
				H *= Evaluate(t);
				
				Mult(H,Vector2D(1.0,1.0),dudq);
				f += dudq;
			}				
		}
	}
	else if (loadtype == Trotatingforce)
	{
		//Trotatingforce ... force applied to a point in direction normal to axis
		//                   two such forces can be used to simulate moment (Kraeftepaar)
		//                   2D: only one axis point needed, axis is in direction Z, node_ax2 can be dummy
		dudq.SetLen(f.GetLen());
		// node numbers of node where force attacs and two nodes on axis
		int node_force, node_ax1, node_ax2;
		double moment;
		GetRotatingForce(moment, node_force, node_ax1, node_ax2);

		if(el->Dim()==3)
		{
			Body3D* bo = (Body3D*)el;
			// v_ax is direction vector of axis (normed)
			Vector3D v_ax;
			v_ax = el->GetNodePos(node_ax2) - el->GetNodePos(node_ax1);
			v_ax.Normalize();
			// r is vector from point on axis to point where force attacs
			Vector3D r;
			r = el->GetNodePos(node_force) - el->GetNodePos(node_ax1);
			// force = moment /  |v x r|^2 (v x r)
			Vector3D force;
			force = v_ax.Cross(r);
			force /= force.Norm2();
			force *= Evaluate(t);

			H.SetSize(f.Length(),el->Dim());

			bo->GetNodedPosdqT(node_force, H); //$ DR 2011-04-14: spec_pos replaced by node_force
			Mult(H, force, dudq);
			f += dudq;
		}
		else
		{
			Body2D* bo = (Body2D*)el;
			// p_ax is point on axis, axis is in Z-direction
			Vector2D p_ax = el->GetNodePos2D(node_ax1);
			// r is vector from point on axis to point where force attacs
			Vector2D r = p_ax - el->GetNodePos2D(node_force);
       
			// force = moment /  |r|^2 (r_hat)
			Vector2D force(-r.Y(), r.X());
			force /= r.Norm2();
			force *= Evaluate(t);

			H.SetSize(f.Length(),el->Dim());

			bo->GetNodedPosdqT(node_force, H);
			Mult(H, force, dudq);
			f += dudq;
		}
	}
	else if (loadtype == Tparameterint)
	{
		*p_int = Evaluate(t);
	}
	else if (loadtype == Tparameterdouble)
	{
		*p_double = Evaluate(t);
	}
	else if (loadtype == Tparametervector3d)
	{
	  *p_vector3d = vecval*Evaluate(t);
	}
}

void MBSLoad::DrawLoad(MBS* mbs, Element* el)
//virtual void MBSLoad::DrawLoad(MBS* mbs) //$ DR 2012-10: loads moved from element to mbs, old code
{
	double size = mbs->GetDOption(120);
	double linethickness = size * 0.04;
	double headsize = size * 0.2;
	Vector3D p(0.,0.,0.); //position of force
	Vector3D dir(0.,0.,0.);

//$!AD 2011-10: loads from IO-Elements are visible as the IO element itself
	if (this->IOelement != 0)
	{
		if(mbs->GetElement(IOelement).DrawElementFlag() == 0)
			return; // dont draw controlled load if control element is not visible
	}

	if (loadtype == TGCload)
	{
		dir = el->GetDOFDirD(gc);
		double val = Evaluate(mbs->GetDrawTime());
		//mbs->UO() << "load=" << val << "\n";
		if (val < 0) dir = -1*dir;
		p = el->GetDOFPosD(gc);
	} 
	else if (loadtype == Tbodyload || loadtype==Tgravity)
	{
		p = el->GetRefPosD();

		if (specpos == 1) {dir = Vector3D(1.,0.,0.);}
		else if (specpos == 2){dir = Vector3D(0.,1.,0.);}
		else if (specpos == 3){dir = Vector3D(0.,0.,1.);}

		double val = Evaluate(mbs->GetDrawTime());
		if (val < 0) dir = -1.*dir;
	}
	else if (loadtype == Tsurfacepressure)
	{
		p = el->GetPosD(Vector3D());

		dir = el->GetSurfaceNormalD(specpos);

		double val = Evaluate(mbs->GetDrawTime());
		if (val < 0) dir = -1.*dir;
	}
	else if ((loadtype == Tforcevector2D) || (loadtype == Tforcevector3D))
	{
		if(el->Dim()==3 || el->Dim()==2)
		{
			Matrix3D A;
			if(node_f > 0)	// DR 2011-05-20: the constructor with node-nr is used
			{
				//forcepos = el->GetNodeLocPos(node_f);
				//forcepos = el->GetNodePosD(node_f);
				p=el->GetNodePosD(node_f);
				if(localBodyAxis) // transform local force to global force
				{
					if (el->Dim()==3) 
					{
						Body3D* bo = (Body3D*)el;
						A = bo->GetRotMatrixD(); // rigid body --> no local position needed
					}
					else
					{
						Body2D* bo = (Body2D*)el;
						A = bo->GetRotMatrix2DD();
					}
				}
			}
			else
			{
				p = el->GetPosD(forcepos);
				dir = vecval;
				if(localBodyAxis)
				{
					if (el->Dim()==3) 
					{
						Body3D* bo = (Body3D*)el;
						if (el->IsRigid())
						{
							A = bo->GetRotMatrixD(); // rigid body --> no local position needed
						}
						else
						{
							A = bo->GetRotMatrixD(forcepos); // rigid body --> no local position needed
						}
						dir = A*dir;
					}
					else
					{
						Body2D* bo = (Body2D*)el;
						A = bo->GetRotMatrix2DD();
						Vector2D	dir2D = Vector2D(vecval(1),vecval(2));
						dir2D = A*dir2D;
						dir = Vector3D(dir2D(1),dir2D(2),0.);
					}
				}
			}

			dir.Normalize();
			double val = Evaluate(mbs->GetDrawTime()*vecval.Norm());
			if (val < 0) dir = -1.*dir;
		}
	}
	else if ((loadtype == Tmomentvector2D) || (loadtype == Tmomentvector3D))
	{
		if(el->Dim()==3 || el->Dim()==2)
		{
			p = el->GetPosD(forcepos);
			dir = vecval;
			if(localBodyAxis && el->Dim()==3) // transform local force to global force
			{
				Matrix3D A;
				Body3D* bo = (Body3D*)el;
				if (el->IsRigid())
				{
					A = bo->GetRotMatrixD(); // rigid body --> no local position needed
				}
				else
				{
					A = bo->GetRotMatrixD(forcepos); // rigid body --> no local position needed
				}
				//Matrix3D A = el->GetRotMatrixD(); // rigid body --> no local position needed
				dir = A*dir;
			}
			dir.Normalize();
			dir *= 2; //showing that this is a moment

			double val = Evaluate(mbs->GetDrawTime()*vecval.Norm());
			if (val < 0) dir = -1.*dir;
		}
	}
	else if (loadtype == Tcentrifugal)
	{
		p = el->GetPos(Vector3D(0.0,0.0,0.0)); // @c.m.
		dir = p - forcepos;
		vecval.GramSchmidt(dir);
		dir.Normalize();
	}
	else if(loadtype == Trotatingforce) //$ DR 2011-04-14
	{
		Vector3D r, v_ax;
		p = el->GetNodePosD(node_f);
		v_ax = el->GetNodePosD(node_2) - el->GetNodePosD(node_1);
		r = el->GetNodePosD(node_f) - el->GetNodePosD(node_1);
		dir = v_ax.Cross(r);
		dir.Normalize();
	}

	else if (loadtype == Tparameterint)
	{
		*p_int = Evaluate(mbs->GetDrawTime());
	}
	else if (loadtype == Tparameterdouble)
	{
		*p_double = Evaluate(mbs->GetDrawTime());
	}
	else if (loadtype == Tparametervector3d)
	{
	  *p_vector3d = vecval*Evaluate(mbs->GetDrawTime()); // val == 1.
	}

	if (loadfunc == TLoadIOElement)
	{
		if(mbs->GetIOption(144))	// do not draw the arrow from control element to load, if control elements are not shown in 3D window
		{
			InputOutputElement* ioe = (InputOutputElement*)mbs->GetElementPtr(IOelement);	
			Vector3D po = ioe->ToP3D(ioe->GetOutputPosD(IOelement_outputnum));
			mbs->MyDrawArrow(po, p, Vector3D(0.3,0.3,0.3));
		}
	}

	if (RelApproxi(dir.Norm(),1.)) //force
	{
		mbs->MyDrawArrow(p + (-1.*size)*dir, p,
			Vector3D(mbs->GetDOption(121),mbs->GetDOption(122),mbs->GetDOption(123)), linethickness, headsize, 6);
	}
	else if (RelApproxi(dir.Norm(),2.)) //moment (torque)
	{
		dir.Normalize();
		mbs->MyDrawArrow(p + (-1.*size)*dir, p,
			Vector3D(mbs->GetDOption(121),mbs->GetDOption(122),mbs->GetDOption(123)), linethickness, headsize, 6);
		mbs->MyDrawArrow(p + (-1.2*size)*dir, p + (-0.2*size)*dir,
			Vector3D(mbs->GetDOption(121),mbs->GetDOption(122),mbs->GetDOption(123)), linethickness, headsize, 6);
	}
}
